#!/usr/bin/python2.7
# -*- coding: utf-8 -*-
import socket
from threading import Thread
import struct
import re
import time
import pickle
import numpy as np
import rospy
import rospkg
from std_msgs.msg import Bool
from enum import Enum
#import aeraimage
from px4_gcs.msg import swarm_state
time_num =1598419990910
global UAV_ID
UAV_ID = 0
global swarm_state_ready
swarm_state_ready = 0

mainclient = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
print('建立客户端套接字')
host = '202.121.181.221'
port = 2333
mainclient.connect((host,port))
print('连接到服务器')

def trj_send():
	#execfile('/home/pengmin/regiolap_ws/src/tcp_comunication/scripts/exp/aeraimage.py')
	rospack = rospkg.RosPack()
	rospack.list()
	pkg_path = rospack.get_path('px4_gcs')
	trj_list = pickle.load(open(pkg_path+'/scripts/exp/trj.pkl', 'rb')) #读取生成的路径文件
	print(len(trj_list),type(trj_list),len(trj_list[0]))
	np.savetxt(pkg_path+'/traj/trjall_recv.csv',trj_list,fmt = '%s',delimiter=',')
	trj_mainclient_raw = trj_list[0]
	trj_mainclient = []
	trj_client1 = []
	trj_client2 = []
	trj0_number = int(len(trj_mainclient_raw)/10)
	init_pos0 = [trj_mainclient_raw[0][0],trj_mainclient_raw[0][1]]
	init_pos1 = [trj_list[1][0][0],trj_list[1][0][1]]
	init_pos2 = [trj_list[2][0][0],trj_list[2][0][1]]
	for i in range(trj0_number):
		trj_mainclient_raw0 = [0,i,trj0_number,trj_mainclient_raw[i*10][0]-init_pos0[0],trj_mainclient_raw[i*10][1]-init_pos0[1]]
		trj_mainclient.append(trj_mainclient_raw0)
	np.savetxt(pkg_path+'/traj/trj0_recv.csv',trj_mainclient,delimiter=',')
	print('写入完毕！！')
	print(len(trj_list[1])/10)
	trj1_number = len(trj_list[1])/10
	trj2_number = len(trj_list[2])/10
	for i in range(trj1_number):
		trj_client1.append(trj_list[1][i*10])
	for i in range(trj2_number):
		trj_client2.append(trj_list[2][i*10])
	send_flag = False 
	client_ID = 1
	for i in range(len(trj_client1)):
		trj_send1 = [client_ID,i+1,len(trj_client1),trj_client1[i][0]-init_pos1[0],trj_client1[i][1]-init_pos1[1]]
		trj_send1_pack = struct.pack('!3i2f',*trj_send1)
		mainclient.send(trj_send1_pack)
		time.sleep(0.05)
	client_ID = client_ID + 1
	for i in range(len(trj_client2)):
		trj_send2 = [client_ID,i+1,len(trj_client2),trj_client2[i][0]-init_pos2[0],trj_client2[i][1]-init_pos2[1]]
		trj_send2_pack = struct.pack('!3i2f',*trj_send2)
		mainclient.send(trj_send2_pack)
		time.sleep(0.05)
	send_flag = True

#def Heartbeat_msg():
# 		heartbeat_msg = [100000,100000,0,0.0,0.0]
# 		heartbeat_msg_pack = struct.pack('!3i2f',*heartbeat_msg)
# 		mainclient.send(heartbeat_msg_pack)
# 		print('Heratbeat message sent!')

# def swarm_state_sub(swarmState):
# 	print('集群状态',swarmState.data)

def mavros_state_sub(mavrosState):
	global swarm_state_ready
	if mavrosState.data == True:
		#这个输入后 在server中的判断要注意区别收到消息的时效性 有可能是client断开了
		state_send = [UAV_ID,1,9999999,0.0,0.0]
		state_send_pack = struct.pack('!3i2f',*state_send)
		mainclient.send(state_send_pack)
		print('无人机 0 已经准备好!')
		state_recv = mainclient.recv(20)
		client_ID,data_number,msg_len,pose_x,pose_y = struct.unpack('!3i2f',state_recv)
		swarm_state_ready = data_number
		print(data_number)
		print(data_number,msg_len)
		if msg_len == 9999999 and data_number == 28:
			print('所有无人机准备好!')
		elif msg_len == 9999999 and data_number == 24:
			print('无人机2未准备好!')
		elif msg_len == 9999999 and data_number == 20:
			print('无人机1未准备好!')
		elif msg_len == 9999999 and data_number == 12:
			print('无人机0未准备好!')
		elif msg_len == 9999999 and data_number == 0:
			print('至少两架无人机未准备好!')
	elif mavrosState.data == False:
		state_send = [UAV_ID,-1,9999999,0.0,0.0]
		state_send_pack = struct.pack('!3i2f',*state_send)
		mainclient.send(state_send_pack)
		print('无人机 0 未准备好!')
		state_recv = mainclient.recv(20)
		client_ID,data_number,msg_len,pose_x,pose_y = struct.unpack('!3i2f',state_recv)
		print(data_number)
		swarm_state_ready = data_number
		if msg_len == 9999999 and data_number == 28:
			print('所有无人机准备好!')
		elif msg_len == 9999999 and data_number == 24:
			print('无人机2未准备好!')
		elif msg_len == 9999999 and data_number == 20:
			print('无人机1未准备好!')
		elif msg_len == 9999999 and data_number == 12:
			print('无人机0未准备好!')
		elif msg_len == 9999999 and data_number == 0:
			print('至少两架无人机未准备好!')

def swarm_state_pub():
	swarm_state_pub = rospy.Publisher('/swarm/state',swarm_state,queue_size = 1)
	rate = rospy.Rate(50)
	swarm_state_msg = swarm_state()
	swarm_state_msg.traj_ready = True
#	swarm_state_list = list(swarm_state.ready)
#	print(swarm_state_list)	
	while len(swarm_state_msg.ready) < 3:
		swarm_state_msg.ready.append(0)
	while len(swarm_state_msg.nodes_id) < 3:
		swarm_state_msg.nodes_id.append(0)
	while not(rospy.is_shutdown()):
		swarm_state_msg.ready[0] = 1 if (swarm_state_ready & 16)==16 else 0# = 1 if (swarm_state_ready & 8)==8 else 0
		swarm_state_msg.ready[1] = 1 if (swarm_state_ready & 8)==8 else 0
		swarm_state_msg.ready[2] = 1 if (swarm_state_ready & 4)==4 else 0
		print(swarm_state_ready,swarm_state_msg.ready[0],swarm_state_msg.ready[1],swarm_state_msg.ready[2])
		swarm_state_msg.master_id = 0
		swarm_state_msg.nodes_id[0] = 0
		swarm_state_pub.publish(swarm_state_msg)
		rate.sleep()
		#print("swarm_state_msg已经发布")
 

#def extend_state_sub(extendState):

	

if __name__ == '__main__':
	trj_send()
	rospy.init_node('mainclient',anonymous = True)
	rate = rospy.Rate(50)
	rospy.Subscriber('/mavros/ready',Bool,mavros_state_sub,queue_size = 1)
	swarm_state_pub()
	ros.spin()
#	rospy.Subscriber('/mavros/ExtenedState',mavros_msgs/ExtenedState Message,extend_state_sub,queuesize = 1)



# U_ID = b'UAV_1'
# client_send_datalist = (b'pose_posin',b'pose_orien',b'set_positn',b'set_orietn',
# 						b'vel_angula',b'vel_linear',b'set_angula',b'set_linear')
# client_send_requflaglist = (b'REQU_ALUAV',b'REQU_UAV_1',b'REQU_UAV_2',b'RE_QUUAV_3',b'REQU_UAV_4',
# 							b'REQU_UAV_5',b'REQU_UAV_6',b'RE_QUUAV_7',b'REQU_UAV_8',b'REQU_UAV_9',
# 							b'REQU_UAV_a',b'REQU_UAV_b',b'RE_QUUAV_c',b'REQU_UAV_d',b'REQU_UAV_e')
# client_send1_fill = b'MSGTYPEFIL'
# client_send2_fill = b'REQURE_FIL'







# while True:
# 	for i in range(5):
# 		client_send_pose = [U_ID,client_send_datalist[0],client_send2_fill,time_num,pose_x,pose_y,pose_z,psoe_fill]
# 		client_send_orientation = [U_ID,client_send_datalist[1],client_send2_fill,time_num,orientation_w,orientation_x,orientation_y,orientation_z]
# 		client_send_posepack = struct.pack('!5s10s10sq4f',*client_send_pose)
# 		print('发送包长度：',len(client_send_posepack))
# 		client_send_orientationpack = struct.pack('!5s10s10sq4f',*client_send_orientation)
# 		client_test.send(client_send_posepack)
# 		client_test.send(client_send_orientationpack)
# 		time.sleep(0.5)
# 	time.sleep(2)
# 	client_require_send = [U_ID,client_send_datalist[1],client_send_requflaglist[2],0,0.0,0.0,0.0,0.0]
# 	print('向服务器请求数据',U_ID,client_send_datalist[1],client_send_requflaglist[2])
# 	client_require_sendpack = struct.pack('!5s10s10sq4f',*client_require_send)
# 	client_test.send(client_require_sendpack)
# 	client_recv_msg = client_test.recv(49)
# 	U_ID_recv,msg_flag_recv,requ_flag_recv,time_num,f_dataa,f_datab,f_datac,f_datad = struct.unpack('!5s10s10sq4f',client_recv_msg)
# 	print('Request后，接收到的客户端消息：',U_ID_recv,msg_flag_recv,requ_flag_recv,time_num,f_dataa,f_datab,f_datac,f_datad)
# #	f_dataa,f_datab,f_datac,f_datad = struct.unpack('!4f',client_recv_msg)
# #	print('接收到的客户端消息：',f_dataa,f_datab,f_datac,f_datad)
# 	time.sleep(5) 


